
#include "pure_pursuit.h"

#include <iostream>
#include <cmath>
#include <algorithm>

void PurePursuitController::FollowThread() {
    while (true) {
        follow_state_ = FollowState::kFollowing;

        // 计算线速度和转向角
        int   closest_idx   = FindClosestPoint();
        int   lookahead_idx = findLookaheadPoint(closest_idx);
        float steer_angle   = CalcSteeringAngle(lookahead_idx);
        float linear_vel    = CalcLinearVel(steer_angle);

        linear_vel_  = linear_vel;
        steer_angle_ = steer_angle;

        // 到达目标，停止跟随
        if (isReachGoal()) {
            follow_state_ = FollowState::kReachGoal;
            // 等待5个控制周期后，将控制器设置成空闲状态
            std::this_thread::sleep_for(std::chrono::milliseconds((int) (control_period_ * 5 * 1000)));
            follow_state_ = FollowState::kIdle;
            break;
        }

        std::this_thread::sleep_for(std::chrono::milliseconds((int) control_period_ * 1000));
    }
}

// 寻找前瞻点
int PurePursuitController::findLookaheadPoint(int start_idx) {
    float k              = 1.0;
    float lookahead_dist = lookahead_dist_ + k * pose_.v;

    for (size_t i = start_idx; i < path_.size(); ++i) {
        double dx   = path_[i].x - pose_.x;
        double dy   = path_[i].y - pose_.y;
        double dist = std::sqrt(dx * dx + dy * dy);

        if (dist >= lookahead_dist) {
            return i;
        }
    }
    return path_.size() - 1;  // 返回最后一个点
}

// 计算steer_angle
float PurePursuitController::CalcSteeringAngle(int lookahead_idx) {
    auto  target_pose = path_[lookahead_idx];
    float dx          = target_pose.x - pose_.x;
    float dy          = target_pose.y - pose_.y;

    // 转换到机器人坐标系
    float local_x = dx * std::cos(pose_.yaw) + dy * std::sin(pose_.yaw);
    float local_y = -dx * std::sin(pose_.yaw) + dy * std::cos(pose_.yaw);

    // 横向误差e(t) = local_y
    // theta = atan(2 * L * e(t) / (x * x + y * y))
    float steer_angle = std::atan(2 * wheel_base_ * local_y / (local_x * local_x + local_y * local_y));

    // 如果是反向的，优先旋转过去
    if (local_x < 0) {
        float angle = std::atan2(local_y, local_x);
        steer_angle = std::copysign(M_PI / 2.0, angle);
    }

    return steer_angle;
}

float PurePursuitController::CalcLinearVel(float steer_angle) {
    float linear_vel =
        std::clamp(max_linear_vel_ - (float) 0.5 * std::abs(steer_angle), min_linear_vel_, max_linear_vel_);

    return linear_vel;
}